/*!
  @file image_subscriber.cpp
  @brief 
  @author Kyohei Otsu <kyon@ac.jaxa.jp>
  @date April, 2013
*/

#include <ros/ros.h>
#include <message_filters/subscriber.h> 
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "mrover_vo/camera_params.h"
#include "mrover_vo/vis_odometer.h"

namespace vo
{

//! class description 
/*! This is a detailed description of the class */
class ImageSubscriber 
{
 public:
   
  //! Constructor
  ImageSubscriber(ros::NodeHandle &nh) 
      : nh_(nh)
      , private_nh_("~")
      , visod_(new VisOdometer)
      , cam_params_(new CameraParams)
  {
    // initializing pub/sub, server
    imgL_sub_.subscribe(nh_, nh_.resolveName("imageL_in"), 5);
    imgR_sub_.subscribe(nh_, nh_.resolveName("imageR_in"), 5);
    sync_.reset(new message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image>(imgL_sub_, imgR_sub_, 10));
    sync_->registerCallback(boost::bind(&ImageSubscriber::stereoImageCB, this, _1, _2));

    cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(nh_.resolveName("cloud_out"), 5);

    //-- initializing optional parameters
    //-- e.g.) private_nh_.param<TYPE>("PARAM_NAME", PARAM_VAR, DEFAULT_VALUE);
    
    // read camera parameters
    std::string camera_yaml;
    private_nh_.param<std::string>("camera_yaml", camera_yaml, "");
    if(camera_yaml.empty()) ROS_ERROR("Please specify 'camera_yaml' parameter.");
    else cam_params_->initFromFile(camera_yaml);
    visod_->setCameraParams(cam_params_);

    // visod_->enableMaskForFeatureDetection();
  }

  //! Destructor
  ~ImageSubscriber()
  {
  }

  //! image callback function
  void stereoImageCB(const sensor_msgs::Image::ConstPtr &, const sensor_msgs::Image::ConstPtr &);

 private:

  void addGaussian(const cv::Mat &src, cv::Mat &dst);

  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;
  message_filters::Subscriber<sensor_msgs::Image> imgL_sub_;
  message_filters::Subscriber<sensor_msgs::Image> imgR_sub_;
  boost::shared_ptr< message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image> > sync_;
  ros::Publisher cloud_pub_;

  VisOdometer::Ptr visod_;
  CameraParams::Ptr cam_params_;

};

void ImageSubscriber::stereoImageCB(
    const sensor_msgs::Image::ConstPtr &imgL_msg, 
    const sensor_msgs::Image::ConstPtr &imgR_msg)
{
  // -- convert image type 
  cv_bridge::CvImage::Ptr cv_ptrL;
  cv_bridge::CvImage::Ptr cv_ptrR;
  try {
    cv_ptrL = cv_bridge::toCvCopy(imgL_msg, "mono8");
    cv_ptrR = cv_bridge::toCvCopy(imgR_msg, "mono8");
  }
  catch(cv_bridge::Exception &e) {
    ROS_ERROR("cv_bridge: conversion error");
    return;
  }

#if 0
  cv::Size hsz(cv_ptrL->image.cols/4, cv_ptrL->image.rows/4);
  cv::Mat imgL(hsz, CV_8U);
  cv::Mat imgR(hsz, CV_8U);
  cv::resize(cv_ptrL->image, imgL, hsz);
  cv::resize(cv_ptrR->image, imgR, hsz);
  visod_->setImages(imgL, imgR);
#endif
  cv::Mat imgL(cv_ptrL->image.size(), CV_8U);
  cv::Mat imgR(cv_ptrR->image.size(), CV_8U);
  addGaussian(cv_ptrL->image, imgL);
  addGaussian(cv_ptrR->image, imgR);
  visod_->setImages(imgL, imgR);
  //visod_->setImages(cv_ptrL->image, cv_ptrR->image);
  visod_->showImages();

  cv::Mat relative_pose(4, 4, CV_32F);
  visod_->compute(relative_pose);
  visod_->showKeyPoints();
  visod_->showMatches();

  sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2);
  visod_->getCloudMsg(*cloud_msg);
  cloud_msg->header.stamp = ros::Time::now();
  cloud_msg->header.frame_id = "/camera_left";
  cloud_pub_.publish(cloud_msg);

  visod_->showTracks(false); // show inliers
  visod_->publishTF();
}

void ImageSubscriber::addGaussian(const cv::Mat &src, cv::Mat &dst) {
  cv::Size ksize(11, 11);
  float sigma = 0;  // automatically computed
  cv::GaussianBlur(src, dst, ksize, sigma);
}

}  // namespace vo

int main(int ac, char **av)
{
  ros::init(ac, av, "ImageSubscriber");
  ros::NodeHandle nh;
  vo::ImageSubscriber handler(nh);
  ros::spin();
  return 0;
}

